classdef HeadingAnalyzer
    %STOPSANALYZER finds stops in a set of X,Y,Z coordinates
    %   Determiness the duration of these stops

    properties
        StopsL={}
        SizeMultiplier = 2;      %multiplier to make stop dots bigger. 
                                 %   easier to see then
                                 
        %constants
        MIN_VELOCITY = .02;         %all velocities under this are considered stops
        VELOCITY_INTERVAL = 1;      %how many points L and R to average for a velocity
                                    %with 1, the velocity at each point
                                    %will be the average of the instant
                                    %velocities before it, and after it
                                    %with 2 it will average the velocities
                                    %between point and 2 points left, and 2
                                    %points right
    end

    methods
        %accessors and getters
        function obj = set.SizeMultiplier(obj, size)
            obj.SizeMultiplier = size;
        end
        
        function obj = set.MIN_VELOCITY(obj, min)
            obj.MIN_VELOCITY = min;
        end
        
      

        
        %Main constructor. Takes x,y,z,t coordinates and finds all stops
        function obj = HeadingAnalyzer(x,y,z,h)
            V = obj.Velocity(x,y,z);
            len = length(V);
            scount = 1;
            start = 0;  
            Xs=0;
            Ys=0;
            hs = [];
            for j=1:len
                %begin timing a stop 
                if V(j) < obj.MIN_VELOCITY && start == 0
                    Xs = x(j);
                    Ys = y(j);
                    hs(1) = h(j);
                    %obj.StopsL{scount} = Stop(h(j), x(j), y(j));
                    start = 1;
                end

                %continue timing a stop
                if V(j) < obj.MIN_VELOCITY && start == 1
                    hs(length(hs)+1) = h(j);
                    h(j)
                    %obj.StopsL{scount}= obj.StopsL{scount}.AddLine(h(j));
                end

                %stop timing a stop and add it to the list
                if V(j) > obj.MIN_VELOCITY && start >0
                    obj.StopsL{scount} = Stop2(hs,Xs, Ys);
                    scount = scount+1;
                    hs = [];
                    start = 0;
                end         
            end
        end

        %main function to plot the Stops
        %hold_on is used to save what was previously in the plot
        %    this is used to overlay things, like stps, floorplans
        %it multiplies the durations by a constant to get bigger dots so
        %   everything is actually visible.
        function Plot(this, hold_on)
           %do not overwrite previous plot, add to it
           if hold_on
               hold on
           end
           
           for j=1:length(this.StopsL)
               if this.StopsL{j}.IsUseful()
                   this.StopsL{j}.Plot();
               end
           end 
        end
        
        
        function list = GetAllTurnInfo(this)
           c = 1;
           list = [];
           for j=1:length(this.StopsL)
               if (this.StopsL{j}.IsUseful())
                   list(c) = this.StopsL{j}.GetInfo();
                   c = c+1;
               end
           end                  
        end
        
        

        %helper function
        %calculates velocity based on the xyz coordinates
        %averages instantaneous velocities across nearby sectors to smooth
        %things out a bit
        function v = Velocity(obj,x,y,z)
            v = zeros(length(x)-2*obj.VELOCITY_INTERVAL, 1);
            INT = obj.VELOCITY_INTERVAL;

            for i=1+INT:length(x)-INT
                vel_back    = distance(x(i), x(i-INT), y(i), y(i-INT),z(i), z(i-INT))/INT;
                vel_forward = distance(x(i), x(i+INT), y(i), y(i-INT),z(i), z(i+INT))/INT;
                v(i-INT) = (vel_back +vel_forward)/2;
            end
        end
    end
end
